#pragma once
#include "pch.h"
#include "Common.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

namespace AutomatedSurfaceReconstruction 
{
	class CPointCloudHandler {
	public:
		CPointCloudHandler() = default;
		~CPointCloudHandler() = default;
		void simplifyPointCloud(CGALPointSetType& vPoints);
		CGALPointSetType process(const std::vector<Point2D>& vPixelSet, CGALPointSetType& vPointCloud, const std::pair<Point2D, Point2D>& vAABB);


	private:
		std::pair<Point2D, Point2D> __getAABB(const CGALPointSetType& vPoints);
		Point2D __map2UV(const Point2D& vPoint, std::pair<Point2D, Point2D> vAABB);
		Point2D __map2PointCloud(const Point2D& vPoint, const std::pair<Point2D, Point2D>& vAABB);
		CGALPointSetType __removePointsInsideBoundary(const std::vector<Point2D>& vBoundaryPointSequence, CGALPointSetType& vPoints);
		float __computeHeightFromNeighbors(const Point2D& vPoint, pcl::KdTreeFLANN<pcl::PointXY>& vKdTree, const CGALPointSetType& vPoints, const std::size_t vNeighborNum = 4);
		std::vector<Kernel::Segment_3> __jointPoint2Segment(const std::vector<Point3D>& vBoundaryPoints);
		CGALPointSetType __addBoundaryPoints(const std::vector<Kernel::Segment_3>& vSegmentSet);
		void __clearBoundaryAround(CGALPointSetType& vPointCloud, const std::vector<Kernel::Segment_3>& vSegmentSet, const float vClearRadius = 3);
	};
}